#include "mission.h"
volatile int Mode7_start_encoder = 0;
volatile int Mode7_end_encoder = 0;
volatile int Mode11_start_encoder = 0;
volatile int Mode11_end_encoder = 0;
volatile float Mode7_ave_Yaw = 0.0f;   // 平均偏航角
volatile float Mode11_back_Yaw = 0.0f; // 反转偏航角
volatile float back_line_Yaw = 0.0f;

extern UART_HandleTypeDef huart2; // 定义UART句柄
extern uint8_t car_mode;          // 车辆模式
extern uint8_t stepper_mode;       // 步进电机模式
extern float base_speed;          // 基础速度
extern float offset;              // 偏移量
void A_mission(void)
{
  if (car_mode == 1) // 题目开始运行，慢速执行等待灰度传感器检测到三个及以上的黑线
  {
    my_set_speed(300, 300);
    uint8_t sum = 0;
    for (int i = 1; i <= 8; i++)
    {
      sum += sensor_buffer[i];
    }
    if ((8 - sum) >= 3) // 检测到初始黑线
    {
      car_mode = 2;             // 进入下一个状态
      buzzer_beep(1, 100, 100); // 蜂鸣器响
      my_set_speed(0, 0);       // 停止电机
    }
  }
  else if (car_mode == 2) // 进入巡线状态
  {
    static uint32_t mode2_start_time = 0;
    if (mode2_start_time == 0) // 第一次进入
    {
      mode2_start_time = uwTick; // 记录开始时间
    }

    __disable_irq(); // 禁用中断，防止在读取编码器时发生中断
    offset = cal_sensor_offset(); // 计算偏移量
    __enable_irq(); // 重新启用中断
    // set - acc
    float Gray_PID_out = PID_encoder_calc(&GW_gray_PID, 0, offset, 0); // 计算灰度传感器PID输出
    // 对于不同的速度进行相应的增益,basespeed取值为300~1000
    float PID_Gain = (base_speed / 300.0f);
    Gray_PID_out *= PID_Gain;
    // 加载在速度环上
    my_set_speed(base_speed - Gray_PID_out, base_speed + Gray_PID_out); // 设置电机速度
    
    uint8_t sum = 0;
    for (int i = 1; i <= 8; i++)
    {
      if (sensor_buffer[i] == 0 || sensor_buffer[i] == 1)
      {
        sum += sensor_buffer[i];
      }
      else 
      {
        sum = 7;
      }
      
    }
    if ((8 - sum) >= 6 && uwTick - mode2_start_time > 10000) // 几乎全黑,到达终点
    {
      car_mode = 3;       // 进入下一个状态
      my_set_speed(0, 0); // 停止电机
    }
  
  }
  else if (car_mode == 3) // 向前走8.0cm
  {

    static uint8_t started = 0; // 改名避免冲突
    static int start_encoder = 0;
    static int target_encoder = 0;

    if (started == 0)
    {
      __disable_irq();     // 禁用中断，防止在读取编码器时发生中断
      Read_ALL_Enconder(); // 获取初始编码器值
      // 取左右编码器平均值作为起点
      start_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2;
      __enable_irq();                                           // 重新启用中断
      target_encoder = start_encoder + distance_to_encoder(8.0f); // 目标编码器值
      started = 1;
      my_set_speed(200, 200); // 设置初始速度
    }
    else if (started == 1)
    {
      __disable_irq(); // 禁用中断，防止在读取编码器时发生中断
      Read_ALL_Enconder();                // 持续读取编码器
      int now_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2;
      // 开启中断
      __enable_irq(); // 重新启用中断
      if (now_encoder >= target_encoder)
      {
        my_set_speed(0, 0); // 到达目标，停止
        // printf("target enc = %d\n", target_encoder);;
        car_mode = 4;
        started = 0; // 重置状态，准备下一次调用
      }
      else if (now_encoder < target_encoder)
      {
        // 角度环修正量
        float angle_correction = PID_Calc(&Yaw_PID, Yaw, Yaw, 1);
        float lspeed = 200 - angle_correction; // 左轮速度
        float rspeed = 200 + angle_correction; // 右轮速度
        my_set_speed(lspeed, rspeed);                // 未到达目标，继续前进
        //printf("no end!   LNOW:%d TARGET%d\n", Encoder_Now[0], target_encoder);
      }
    }
  }
  else if (car_mode == 4) // 到达发车点
  {
    static uint8_t Yaw_cnt = 0;
    static float target_Yaw = 0.0f; // 目标偏航角
    if (target_Yaw == 0.0f)
    {
      target_Yaw = Yaw + 90.0f;
      // 归一化
      if (target_Yaw > 180.0f)
      {
        target_Yaw -= 360.0f; // 确保目标角度在 -180 到 180 度之间
      }
      else if (target_Yaw < -180.0f)
      {
        target_Yaw += 360.0f; // 确保目标角度在 -180 到 180 度之间
      }
    }
    if (Yaw > target_Yaw - 1.0f && Yaw < target_Yaw + 1.0f) // 偏航角误差小于1度
    {
      Yaw_cnt++; // 增加计数器
    }
    else
      Yaw_cnt = 0;

    if (Yaw_cnt >= 100)
      car_mode = 5;                                             // 100次循环后，已经稳定，进入下一个状态
    float control_val = PID_Calc(&Yaw_PID, target_Yaw, Yaw, 1); // 计算偏航角PID输出
    my_set_speed(-control_val, control_val);                    // 设置电机速度
  }
  else if (car_mode == 5) // 分别向左45度，中间，和右45度转向，遇到红色色块立马跳转下一个状态
  {
    static float mode5_target_Yaw = 45.0f; // 目标偏航角
    static uint8_t turn_cnt = 0;
    static uint16_t delay_cnt = 0; // 延时计数器，作用域提升

    // 按照turn_cnt选择目标角度
    if (turn_cnt == 0)
    {
      mode5_target_Yaw = 45.0f; // 左转45度
    }
    else if (turn_cnt == 1)
    {
      mode5_target_Yaw = 0; // 中间
    }
    else if (turn_cnt == 2)
    {
      mode5_target_Yaw = -45.0f; // 右转45度
    }
    else
    {
      // 已经完成所有转向，可以进入下一个状态
      car_mode = 6;
      turn_cnt = 0;  // 复位计数器（可选）
      delay_cnt = 0; // 复位延时计数器
      return;
    }

    // 判断是否到达目标角度
    if (Yaw > mode5_target_Yaw - 5.0f && Yaw < mode5_target_Yaw + 5.0f) // 偏航角误差小于5度
    {
      // 到达目标后，等待一小段时间再进入下一个目标（防止抖动，可以加个延时计数器）
      delay_cnt++;
      if (delay_cnt > 20)
      {             // 例如连续20次满足才认为到达
        turn_cnt++; // 切换到下一个目标
        delay_cnt = 0;
      }
    }
    else
    {
      // 只要不满足目标，就复位延时计数器
      delay_cnt = 0;
    }

    if (is_findblob) // 遇到红色色块
    {
      car_mode = 6; // 跳转到下一个状态
      turn_cnt = 0;
      delay_cnt = 0;
      return;
    }
    float control_val = PID_Calc(&Yaw_PID, mode5_target_Yaw, Yaw, 1); // 计算偏航角PID输出
    my_set_speed(-control_val, control_val);                          // 设置电机速度
  }
  else if (car_mode == 6) // 到达发车点
  {

    static uint8_t find_blobs_cnt = 0;
    // 开始对准色块
    float control_val = PID_Calc(&Find_blobs_PID, (float)(CAM_X_WIDE / 2), x, 0); // 计算色块位置PID输出
    my_set_speed(-control_val, control_val);                                      // 设置电机速度
    if (x >= (CAM_X_WIDE / 2 - 40) && x <= (CAM_X_WIDE / 2 + 40))                 // 色块位置在WIDE±40范围内
    {
      find_blobs_cnt++; // 增加计数器
    }
    else
      find_blobs_cnt = 0; // 否则重置计数器
    if (find_blobs_cnt >= 100)
    {
      car_mode = 7;
    }
  }
  else if (car_mode == 7) // 已经对准了色块位置
  {
    // printf("get:%d\n",get_Nowencoder_value());
    if (Mode7_start_encoder == 0 && Mode7_ave_Yaw == 0.0f) // 如果还没有记录开始编码器值和平均偏航角
    {
      Mode7_start_encoder = (int)get_Nowencoder_value(); // 记录当前编码器值
      Mode7_ave_Yaw = Yaw;                               // 记录当前偏航角
      buzzer_beep(1, 100, 100);                          // 蜂鸣器响
    }

    // 向色块前进
    if (blob_area < BLOB_TARGET_AREA) // 色块面积小于目标面积
    {
      Mode7_ave_Yaw = (Mode7_ave_Yaw * 0.9f) + (Yaw * 0.1f); // 记录平均偏航角
      // BT_printf("ave_Yaw = %f\n", ave_Yaw);
      // BT_printf("Yaw = %f\n", Yaw);
      float control_val = PID_Calc(&Go_Target_PID, (float)(CAM_X_WIDE / 2), x, 0);
      float scale = 2.0f / (sqrtf((float)blob_area) + 10.0f); // k1=2.0, k2=10.0 可调
      if (scale > 1.0f)
        scale = 1.0f;
      if (scale < 0.2f)
        scale = 0.2f;
      my_set_speed(BLOB_BASE_SPEED - control_val * scale, BLOB_BASE_SPEED + control_val * scale); // 前进
    }
    else // 色块面积大于目标面积
    {
      my_set_speed(0, 0);                              // 停止
      Mode7_end_encoder = (int)get_Nowencoder_value(); // 记录当前编码器值
      buzzer_beep(1, 100, 100);                        // 蜂鸣器响
      car_mode = 8;

      // 进入下一个状态
    }
  }
  else if (car_mode == 8) // 到达目标色块,识别色块旁的数字，并且使得蜂鸣器鸣笛相应的次数
  {
    if (hash_table[0] == 0)
    {
      yolo_flag = 0x01;
      HAL_UART_Transmit(&huart2, &yolo_flag, 1, HAL_MAX_DELAY);
    }
    else if (hash_table[0] > 30) // 如果经历了30次以上的采样
    {
      car_mode = 9;     // 进入下一个状态
      yolo_flag = 0x00; // 关闭YOLO检测
      HAL_UART_Transmit(&huart2, &yolo_flag, 1, HAL_MAX_DELAY);
    }
  }
  else if (car_mode == 9) // 统计蜂鸣器鸣笛相应的次数
  {
    static uint8_t beep_cnt = 1;
    for (int i = 1; i <= 9; i++)
    {
      if (hash_table[i] > hash_table[beep_cnt]) // 找到最大值
      {
        beep_cnt = i; // 记录对应的数字
      }
    }
    if (beep_cnt > 0 && beep_cnt <= 9) // 如果数字在1~9之间
    {
      buzzer_beep(beep_cnt, 100, 100); // 蜂鸣器鸣笛相应的次数
      car_mode = 10;                   // 进入下一个状态
    }
  }
  else if (car_mode == 10) // 开始返回
  {
    static float back_Yaw = 0.0f; // 反转偏航角
    static uint8_t back_cnt = 0;  // 反转计数器
    // 根据ave_Yaw为基础反转180度，并且归一化
    if (back_Yaw == 0.0f)
    {
      back_Yaw = Mode7_ave_Yaw + 180.0f; // 反转180度
      if (back_Yaw > 180.0f)
      {
        back_Yaw -= 360.0f; // 确保目标角度在 -180 到 180 度之间
      }
      else if (back_Yaw < -180.0f)
      {
        back_Yaw += 360.0f; // 确保目标角度在 -180 到 180 度之间
      }
    }
    float control_val = PID_Calc(&Yaw_PID, back_Yaw, Yaw, 1); // 计算偏航角PID输出
    my_set_speed(-control_val, control_val);                  // 设置电机速度
    if (Yaw > back_Yaw - 5.0f && Yaw < back_Yaw + 5.0f)       // 偏航角误差小于1度
    {
      back_cnt++; // 增加计数器
    }
    else
      back_cnt = 0;
    if (back_cnt >= 30) // 100次循环后，已经稳定，进入下一个状态
    {
      car_mode = 11;              // 进入下一个状态
      my_set_speed(0, 0);         // 停止电机
      Mode11_back_Yaw = back_Yaw; // 记录反转偏航角

      // printf("Yaw = %f\n", Yaw);
    }
  }
  else if (car_mode == 11) // 返回引导线
  {

    if (Mode11_start_encoder == 0)
    {
      Mode11_start_encoder = get_Nowencoder_value();
      Mode11_end_encoder = Mode11_start_encoder + (Mode7_end_encoder - Mode7_start_encoder);
      // buzzer_beep(1, 100, 100);
      //  printf("Mode11_back_Yaw = %f\n", AmissionNode.Mode11_back_Yaw);
      //  printf("Yaw = %f\n", Yaw);
    }

    // 向前走使得encoder增加deltaencoder
    if (get_Nowencoder_value() < Mode11_end_encoder) // 如果编码器值小于结束值
    {
      // 根据Yaw_PID计算偏航角PID输出
      float control_val = PID_Calc(&Yaw_PID, Mode11_back_Yaw, Yaw, 1);  // 计算偏航角PID输出
      my_set_speed(base_speed - control_val, base_speed + control_val); // 设置电机速度
    }
    else if (get_Nowencoder_value() >= Mode11_end_encoder) // 如果编码器值大于等于结束值
    {
      my_set_speed(0, 0); // 停止电机
      car_mode = 12;      // 进入下一个状态
    }
  }
  else if (car_mode == 12) // 返回到发车点
  {
    // 角度环使得Yaw为90度
    static uint8_t mode12_cnt = 0;                                        // 计数器
    static float mode12_target_Yaw = 90.0f;                               // 目标偏航角
    float control_val = PID_Calc(&Yaw_PID, mode12_target_Yaw, Yaw, 1);    // 计算偏航角PID输出
    my_set_speed(-control_val, control_val);                              // 设置电机速度
    if (Yaw > mode12_target_Yaw - 5.0f && Yaw < mode12_target_Yaw + 5.0f) // 偏航角误差小于5度
    {
      mode12_cnt++; // 增加计数器
    }
    else
      mode12_cnt = 0;      // 否则重置计数器
    if (mode12_cnt >= 100) // 100次循环后，已经稳定，进入下一个状态
    {
      car_mode = 13;            // 进入下一个状态
      my_set_speed(0, 0);       // 停止电机
      buzzer_beep(1, 100, 100); // 蜂鸣器响
      // printf("Yaw = %f\n", Yaw);
    }
  }
  else if (car_mode == 13)
  {
    // 巡线
    uint8_t sum = 0;
    for (int i = 1; i <= 8; i++)
    {
      if (sensor_buffer[i] == 0 || sensor_buffer[i] == 1)
      {
        sum += sensor_buffer[i];
      }
      else 
      {
        sum = 7;
      }
    }
    sum = 8 - sum;
    if (sum >= 5) // 停车
    {
      my_set_speed(0, 0);       // 停止电机
      car_mode = 14;            // 进入下一个状态
      buzzer_beep(1, 100, 100); // 蜂鸣器响
    }
    else // 巡线
    {
      __disable_irq(); // 禁用中断，防止在读取编码器时发生中断
      offset = cal_sensor_offset(); // 计算偏移量
      __enable_irq(); // 重新启用中断
      float Gray_PID_out = PID_encoder_calc(&GW_gray_PID, 0, offset, 0); // 计算灰度传感器PID输出
      float PID_Gain = (base_speed / 300.0f);
      Gray_PID_out *= PID_Gain;
      my_set_speed(base_speed - Gray_PID_out, base_speed + Gray_PID_out); // 设置电机速度
    }
  }
  else if (car_mode == 14) // 向前走20cm使得小车完全归位
  {
    static uint8_t started = 0; // 改名避免冲突
    static int start_encoder = 0;
    static int target_encoder = 0;

    if (started == 0)
    {
      __disable_irq();     // 禁用中断，防止在读取编码器时发生中断
      Read_ALL_Enconder(); // 获取初始编码器值
      // 取左右编码器平均值作为起点
      start_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2;
      __enable_irq();                                           // 重新启用中断
      target_encoder = start_encoder + distance_to_encoder(20.0f); // 目标编码器值
      started = 1;
      my_set_speed(200, 200); // 设置初始速度
    }
    else if (started == 1)
    {
      // 关中断
      __disable_irq(); // 禁用中断，防止在读取编码器时发生中断
      Read_ALL_Enconder();                // 持续读取编码器
      int now_encoder = (Encoder_Now[0] + Encoder_Now[3]) / 2;
      // 开启中断
      __enable_irq(); // 重新启用中断
      if (now_encoder >= target_encoder)
      {
        my_set_speed(0, 0); // 到达目标，停止
        // printf("target enc = %d\n", target_encoder);;
        car_mode = 15;
        started = 0; // 重置状态，准备下一次调用
      }
      else if (now_encoder < target_encoder)
      {
        // 角度环修正量
        //float angle_correction = PID_Calc(&Yaw_PID, -180.0, Yaw, 1);
        float lspeed = 200; // 左轮速度
        float rspeed = 200 ; // 右轮速度
        my_set_speed(lspeed, rspeed);          // 未到达目标，继续前进
        //printf("no end!   LNOW:%d TARGET%d\n", Encoder_Now[0], target_encoder);
      }
    }
  }
}
void Stepper_Motor_Mission(void)
{
  if (stepper_mode == 1)
  {
    
  }

}